/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotCode extends IterativeRobot {
    public PhoenixLifter lifter;
    public PhoenixArm arm;
    public PhoenixSensors sensors;
    public PhoenixDrive drive;
    public PhoenixControls controls;
    public PhoenixAutonomous autonomous;
    public Compressor comp;
    public PhoenixBcd autonomousSwitch;
    public PhoenixTeleop teleop;
    public PhoenixDebug debug;
    public PhoenixTesting testing;
    public PhoenixInit init;
    public PhoenixMinibot minibot;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        init = new PhoenixInit(PhoenixInit.RobotType.bot2010);
        
        comp = init.compressor;
        sensors = new PhoenixSensors(init.lightLeft, init.lightMiddle,
                init.lightRight);
        controls = new PhoenixControls(init.leftStick, init.rightStick,
                init.gamePad, init.constants);
        drive = new PhoenixDrive(init.frontLeftMotor, init.rearLeftMotor,
                init.frontRightMotor, init.rearRightMotor, init.leftEncoder,
                init.rightEncoder, init.constants);
        lifter = new PhoenixLifter(init.lifterMotor, sensors, controls,
                init.lifterPotentiometer, init.lifterTopLimit,
                init.lifterBottomLimit, init.constants);
        arm = new PhoenixArm(init.clawMotor, init.clawTopLimit,
                init.clawBottomLimit, init.clawPneumatic,
                init.clawPotentiomemter, init.constants);
        autonomousSwitch = new PhoenixBcd(init.bcd1, init.bcd2);
        autonomous = new PhoenixAutonomous(drive, lifter, sensors, arm);
        minibot = new PhoenixMinibot(init.minibotPneumatic);
        teleop = new PhoenixTeleop(controls, lifter, drive, arm, minibot,
                init.constants);
        debug = new PhoenixDebug(drive, controls, sensors, lifter, arm,
                autonomousSwitch, autonomous, minibot);
        testing = new PhoenixTesting(drive, controls, sensors, lifter, arm,
                autonomousSwitch, autonomous);

        AxisCamera camera = AxisCamera.getInstance();
        camera.writeMaxFPS(30);
                
        comp.start();
    }

    long startTime;
    boolean doneMoving;

    public void autonomousInit() {
        doneMoving = false;
        autonomous.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        PhoenixAutonomous.AutonomousMode mode;
        // motor power level when moving
        double movementScale = -.6;
        switch(autonomousSwitch.get()) {
            case 0:
                autonomous.addMessage("switch set at 0, not running");
                mode = null;
                return;
            case 1:
                autonomous.addMessage("switch set at 1, running straight");
                mode = PhoenixAutonomous.AutonomousMode.kStraight; break;
            case 2:
                autonomous.addMessage("switch set at 2, running left turn");
                mode = PhoenixAutonomous.AutonomousMode.kForkRight; break;
            case 3:
                autonomous.addMessage("switch set at 3, running right turn");
                mode = PhoenixAutonomous.AutonomousMode.kForkLeft; break;
            default:
                autonomous.addMessage("weird switch value, disabling");
                mode = null;
        }

        if(mode != PhoenixAutonomous.AutonomousMode.kStraight &&
                mode != PhoenixAutonomous.AutonomousMode.kForkRight &&
                mode != PhoenixAutonomous.AutonomousMode.kForkLeft) {
            return;
        } else {
            autonomous.updateStage(mode);
            autonomous.run(movementScale);
        }
        debug.autonomous();
        debug.encoders();
        debug.print();
    }

    public void teleopInit() {
        teleop.start();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {

        // feed the watchdog (not sure if different process is needed after
        // update)
        Watchdog.getInstance().feed();
        //PhoenixControls.LifterValues lifterValues = controls.gamepadLifter();
        

        // drive and lifter code
        teleop.updateDrive();
        teleop.updateLifter();
        teleop.updateMinibot();

        debug.lifter();
        debug.arm();
        debug.lightSensors();
        debug.print();
    }
}
